#
#	
#	GrouhBotLib ( GBLib )
#	---------------------	
#	LICENCE: GPLv2
#
require 'robobuilder'

# IDs des servos -----------------------------------------------------
L_TOR = 10 #torso                  ___
L_SHO = 11 #shoulder	       __ [°_°] __  
L_ARM = 12 #arm               [RS]RT|LT[LS] 
           #                  [RA]RH|LH[LA]
R_TOR = 13 #torso                |RT|LT|
R_SHO = 14 #shoulder             [RK|LK] 
R_ARM = 15 #arm                RF[RA|LA]LF

L_HIP = 0 #hip(hanche)
L_THI = 1 #thigh (cuisse)
L_KNE = 2 #knees (genoux)
L_ANK = 3 #anles (chevilles)
L_FOO = 4 #foot

R_HIP = 5 #hip
R_THI = 6 #thigh
R_KNE = 7 #knees
R_ANK = 8 #ankles
R_FOO = 9 #foot

# IDs des boutons de la télécommande IR ------------------------------
# pour 0, id = 21          _____________
# pour 1..9, id = n+11  #/\_____\oo\_____ \
IR_A = 1		# / (A)      (B)   \
IR_B = 2		#| TOPL  TOP  TOPR  \
IR_UP = 4		#|LEFT   STOP  RIGHT|
IR_DOWN = 10 		#\  BOTL BOT BOTR   /
IR_LEFT = 6		# \ 1     2     3  /
IR_RIGHT = 8		# | 4     5     6  |
IR_TOPLEFT = 3 		# | 7     8     9  |
IR_TOPRIGHT = 5		#\| *     0     #  |
IR_BOTLEFT = 9		# \______________ /
IR_BOTRIGHT = 11
IR_STOP = 7

# Autre --------------------------------------------------------------
#Header en mode DC ou DCMP 
WCK_HEADER = 0xFF

# ID pour les commandes DCMP
DCMP_ID = 30

# Position "basic" HUNO
BASIC = [143,179,198,83,106,106,69,48,167,141,47,47,49,199,204,204]

class GrouhBot < Robobuilder

	# Propriétés
	# -----------------------------------------------------------------------------------
	@@init_pos = false
	@@sids = [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 ]
	@@pos = []
	@@kfactor = 1.0
	# -----------------------------------------------------------------------------------

	def getPos
		@@pos
	end

	# - Movements interpolations
	# ----------------------------------------------------------------------------------
	# Different type of move interpolation
	# from http://robosavvy.com/forum/viewtopic.php?t=5306&start=30
	# orinial by RN1AsOf091407

	# MoveTypes:
	# ------------
	#  :accelDecel, 
	#  :accel,
	#  :decel,
	#  :linear 

	def calculatePos_AccelDecel( dst, fractionOfMove = 0.25)
		#accel
		if fractionOfMove < 0.5 
		    return calculatePos_Accel(dst.to_f / 2.0, fractionOfMove * 2.0)

		#decel
		elsif fractionOfMove > 0.5 	
		    dst = dst.to_f
  		    return calculatePos_Decel(dst/2.0, (fractionOfMove - 0.5) * 2.0) + dst * 0.5

		else  #= .5! Exact Middle.
		    return distance / 2
		end
	end

	def calculatePos_Accel(dst, fractionOfMove) 
		fractionOfMove * (dst.to_f * fractionOfMove)
	end

	def calculatePos_Decel(dst, fractionOfMove)
		fractionOfMove = 1 - fractionOfMove;
		dst.to_f - (fractionOfMove * (dst.to_f * fractionOfMove))
	end

	def calculatePos_Linear( dst, fractionOfMove)
		dst.to_f * fractionOfMove
	end

	def getMoveValue( mt, startPos, endPos, fractionOfMove)
		if startPos > endPos

			dist = startPos - endPos
			offset = endPos
			case mt
				when :accel 
				  return dist - calculatePos_Accel(dist, fractionOfMove) + offset

				when :accelDecel
				  return dist - calculatePos_AccelDecel(dist, fractionOfMove) + offset

				when :decel
				  return dist - calculatePos_Decel(dist, fractionOfMove) + offset

				when :linear
				  return dist - calculatePos_Linear(dist, fractionOfMove) + offset
			end
		else
			dist = endPos - startPos
			offset = startPos

			case (mt)
	 			when :accel
				  return calculatePos_Accel(dist, fractionOfMove) + offset

				when :accelDecel
				  return calculatePos_AccelDecel(dist, fractionOfMove) + offset

				when :Decel
				  return calculatePos_Decel(dist, fractionOfMove) + offset

				when :linear
				  return calculatePos_Linear(dist, fractionOfMove) + offset
			end
		end
		return 0.0
	end
	
	# ----------------------------------------------------------------------------------
	# Read all servos pos in @@pos array
	def servoID_readservo( num = 0 )
		@@pos = Array.new( num == 0 ? @@sids.count : num )
		@@sids.each do |id|
			if id >= 0
				p = wckReadPos(id)[1]
				if p < 255
					@@pos[id] = p
				else
					@@pos[id] = -1
					puts "WARNING: servo #{id} returned pos #{p}"
				end
			end
		end
		@@init_pos = true;
		@@init_pos
	end

	# return the given standard (non-DC/DCMP) command and data packet
	# type 		command type
	# *contents 	command data
	def getCommand( type, *contents )
		return HEADER + [ type, 0x00, contents.size ].pack( 'CCN' ) +
		      contents.pack( 'C' * contents.size ) + [ checksum( *contents ) ].pack( 'C' )
	end
	
	# write the given standard (non-DC/DCMP) command to serial port
	# without expecting a response
	# type		command type
	# *contents	command data
	def nr_command( type, *contents )
		return write(getCommand( type, *contents ));
	end
	

	# send a built-in motion command w/o response
	# n	motion ID
	def nr_motion( n )
		sleep DELAY; nr_command( 0x14, n )
	end 

	# send a buil-in motion w/o response
	# n	motion number
	def nr_run( n )
		case n 
		when 1 .. 10
			nr_motion n + 11
		when 11 .. 20
			nr_motion n + 22
		else
			raise "Program number must be in 1 .. 20 (was #{n})"
		end
	end

	# run a built-in motion as with the remote ir ------------------------------

	def a
		nr_motion GETUP_A
	end

	def b
		nr_motion GETUP_B
	end

	def turn_left
		nr_motion TURN_LEFT
	end

	def forward
		nr_motion MOTION_FORWARD
	end

	def turn_right
		nr_motion TURN_RIGHT
	end

	def left
		nr_motion MOVE_LEFT
	end

	def basic
		nr_motion MOTION_BASIC_POSTURE
	end

	def rinht
		nr_motion MOVE_RIGHT
	end

	def attack_left
		nr_motion ATTACK_LEFT
	end

	def backward
		nr_motion MOVE_BACKWARD
	end

	def attack_right
		nr_motion ATTACK_RIGHT
	end

	# --------------------------------------------------------------------------------

	# return (w/o sending to serial port) a wck command packet (DC/DCMP only)
	# data1		1st data byte
	# data2		2nd data byte
	def _wckCmd( data1, data2 )
		buff = Array.new(4)
		buff[0] = WCK_HEADER
		buff[1] = data1
		buff[2] = data2
		buff[3] = wckChecksum( buff[1], buff[2] )
		return buff.pack('CCCC')
	end 

	# Begin DCMP Commands ------------------------------------------------------------

	# read DCMP version
	def dcmpReadVersion
		wckDCMPCommand 0
	end 
	
	# return [y,z] values of accel.
	def dcmpYZ
		wckDCMPCommand 1
	end

	# return [x,z] values of accel.
	def dcmpXZ
		wckDCMPCommand 2
	end

	# read [x,y,z] values of accel.
	def dcmpAccelerometer
		yz = dcmpYZ
		xz = dcmpXZ 
		[ xz[0], yz[0], yz[1] ]
	end 

	# set PSD Sensor On/Off or read its state
	def dcmpPSD( flag = 0x0F )
		if flag == 0x0F		# Read state
			return wckDCMPCommand(5)[0]
		elsif flag == true 	# Activate
			return wckDCMPCommand 3
		elsif flag == false 	# DeActivate
			return wckDCMPCommand 4
		else
			raise Exception.new "Mauvaise valeur pour dcmpPSD, doit être: false, true ou rien pour lire."	
		end
		nil
	end

	# Read Voltage
	def dcmpReadVoltage
		wckDCMPCommand 6
	end

	# read InfraRed
	def dcmpReadIR
		wckDCMPCommand 7
	end
	
	# Set sound sampling On/Off
	def dcmpSetSampling( flag )
		flag ? wckDCMPCommand(8) : wckDCMPCommand(9)
	end
	
	# Returns the current sound buffer - 2 bytes, average of last 8 samples (4ms each)
	def dcmpReadSound 
		wckDCMPCommand 10
	end

	# robot's uptime [sec:min]
	def dcmpUptime
		wckDCMPCommand 11
	end

	# Sample mic level 50 times and returns average
	def dcmpGetMicLevel
		wckDCMPCommand 12
	end

	# ????..
	# @TODO Ne Fonctionne pas, 
	# et ne sais pas comment
	# doit fonctionner..
	# ????..
	def dcmpReadSensors()
		wckDCMPCommand 15
	end

	# Read the current sound Buffer
	# 1st byte => n of bytes
	# Then => bytes
	def dcmpSoundBuffer
		write _wckCmd((5 << 5 | (DCMP_ID % 31)), 16)
		len = read(1).unpack('C')[0]
		fmt = ''
		len.times { fmt += 'C' } 
		return read(len).unpack( fmt )
	end 

	# End DCMP Commands --------------------------------------------------------------
	
	# write a DCMP wck command to serial port
	# c 		command ID
	# length	response length
	# fmt		response format
	def wckDCMPCommand( c, length= 2, fmt = 'CC' ) 
		write _wckCmd((5 << 5 | (DCMP_ID % 31)), c ); return read( length ).unpack( fmt )
	end

	# return checksum of data1 and data2
	def wckChecksum( data1, data2 )
		( data1 ^ data2 ) & 0x7F
	end

	# read servo position in degree / 360
	def wckReadPosDeg( id ) 
		((wckReadPos(id)[1].to_f / 255.0) * 360.0).floor
	end 

	# read servo position /255
	def wckReadPos( id ) 
		write _wckCmd((5 << 5 | (id % 31 )), 0 ); return read(2).unpack('CC')
	end

	# put servo in passive mode.
	# 0x10 => mode 1
	def wckPassive( id, mode = 0x10)	
		write _wckCmd((6 << 5 | (id % 31)), mode); return read(2).unpack('CC')[1]
	end 
	
	# move servo to specified pos in degree / 360
	# id	servo id
	# pos	position (/360)
	# torq	torq
	def wckMovePosDeg( id, pos, torq = 1)
		return wckMovePos( id, ((pos.to_f/360.0)*255.0).floor, torq )
	end 
	
	# move servo to specified pos
	# id	servo id
	# pos	position (/255)
	# torq	torq
	def wckMovePos( id,  pos, torq = 1)
		write _wckCmd( (((torq % 5) << 5) | (id % 31)), (pos % 254)); return read(2).unpack('n').first
	end

	# puts all robot's servo in mechanical 
	# break mode
	def wckBreak
		write _wckCmd(((6 << 5) | 31), 0x20); return read(2).unpack('n')
	end

	# Move to position (multiple servos sync)
	# ------------------------------------------------------------------
	def syncPosSend( lastID, speedLevel, targetArray, index )
		cmd = _syncPosSend( lastID, speedLevel, targetArray, index)
		write cmd
		#@TODO
		# return read( cmd.length ) ou un truc dans le genre ?...
	end
	
	def _syncPosSend( lastID, speedLevel, targetArray, index)
		buff = Array.new( 5 + lastID )
		i = 0
		checkSum = 0
		buff[0] = 0xFF
		buff[1] = ((speedLevel << 5) | 0x1F)
		buff[2] = lastID + 1

		while true
			break if i > lastID
			buff[3 + i] = targetArray[index * (lastID + 1) + i]
			checkSum ^= targetArray[index * (lastID + 1) + i]
			i+=1
		end

		checkSum = checkSum & 0x7F
		buff[3 + i] = checkSum
		# now output buff[]
		# Debug info :: for (i = 0; i < buff.Length - 1; i++) 
		#    Console.Write(buff[i] + ","); Console.WriteLine(buff[i]);

		packFmt = ''
		buff.count().times { packFmt += 'C' }
		return buff.pack( packFmt )
	end
	private :_syncPosSend
	
	# Set servo operations
	# ------------------------------------------------------------------

	# write op command
	def wckSetOper(d1, d2, d3, d4)
		buff = Array.new(6)
		buff[0] = 0xFF
		buff[1] = d1
		buff[2] = d2
		buff[3] = d3
		buff[4] = d4
		buff[5] = (buff[1] ^ buff[2] ^ buff[3] ^ buff[4]) & 0x7f

		write buff.pack('CCCCCC')
		return read(2).unpack('n').first
	end

	# Set communication baudrate
	# values for baudrate:
	# 	0(921600bps), 1(460800bps), 3(230400bps), 7(115200bps),
	# 	15(57600bps), 23(38400bps), 95(9600bps), 191(4800bps),
	def wckSetBaudRate( baudrate, id )
		d1 = (7 << 5) | (id % 31)

		case baudrate
			when 115200
				d3 = 7
			when 57600
				d3 = 15
			when 9600
				d3 = 95
			when 4800
				d3 = 191
			else
				return false;
		end
		return wckSetOper( d1, 0x08, d3, d3 )
	end

	# Set servo speed
	def wckSetSpeed( id, speed, acceleration )
		return false if speed < 0 || speed > 30
		return false if acceleration < 20 || acceleration > 100
			
		d1 = (7 << 5) | (id % 31)
		d3 = speed
		d4 = acceleration
		return wckSetOper(d1, 0x0D, d3, d4)
	end
	
	# set servo overload
	# values for overload:
	# 	 1: 33 400
	# 	 2: 44 500
	# 	 3: 56 600
	# 	 4: 68 700
	# 	 5: 80 800
	# 	 6: 92 900
	# 	 7: 104 1000
	# 	 8: 116 1100
	# 	 9: 128 1200
	# 	10: 199 1800
	def wckSetOverload( id, overT )
		d3 = 33
		d1 = (7 << 5) | (id % 31)
		case overT
			when 400
				d3 = 33
			when 500
				d3 = 44
			when 600
				d3 = 56
			when 700
				d3 = 68
			when 800
				d3 = 80
		end
		return wckSetOper( d1, 0x0F, d3, d3 )
	end

	#
	# wck - Read operation(s)
	# 
	def wckReadPDgain( id )
		wckSetOper((7 << 5) | (id % 31), 0x0A, 0x00, 0x00)
	end

	def wckReadIgain( id )
		wckSetOper((7 << 5) | (id % 31), 0x16, 0x00, 0x00)
	end

	def wckReadSpeed( id )
		wckSetOper((7 << 5) | (id % 31), 0x0E, 0x00, 0x00)
	end

	def wckReadOverload( id )
		wckSetOper((7 << 5) | (id % 31), 0x10, 0x00, 0x00)
	end

	def wckReadBoundary( id )
		wckSetOper((7 << 5) | (id % 31), 0x12, 0x00, 0x00)
	end

	def wckReadIO( id )
		wckSetOper((7 << 5) | (id % 31), 0x65, 0x00, 0x00)
	end

	def wckReadMotionData( id )
		wckSetOper((7 << 5) | (id % 31), 0x97, 0x00, 0x00)
	end

	def wckPosRead10Bit( id )
		wckSetOper((7 << 5), 0x09, id, id)
	end


	#TODO:# **************************************************************************************
	# I2C hardware functions (require DCMP)
	# ********************************************************************************************
	# ---------------------------------------
	# voir à quoi ca sert et
	# 	 si c'est tjrs utile en ruby,
	#	 et si oui si c'est bon comme ça
	# ---------------------------------------
	#def cbyte( b )
	#	if b > 127
	#		i = b - 256 
	#	else
	#		i = b
	#	end 
	#	return i
	#end
end
